﻿#include "harrisdetector.h"

HarrisDetector::HarrisDetector()
    : m_neighbourhood(3),
      m_aperture(3),
      m_k(0.1),
      m_maxStrength(0.0),
      m_threshold(0.01), m_nonMaxSize(3)
{
          setLocalMaxWindowSize(m_nonMaxSize);
}

void HarrisDetector::setLocalMaxWindowSize(int size)
{
      m_nonMaxSize = size;
      m_kernel.create(m_nonMaxSize, m_nonMaxSize, CV_8U);
}

void HarrisDetector::detect(const cv::Mat& image)
{
      // Harris computation
      cv::cornerHarris(image, m_cornerStrength,
                 m_neighbourhood,// neighborhood size
                 m_aperture,     // aperture size
                 m_k);           // Harris parameter

      // internal threshold computation
      double minStrength; // not used
      cv::minMaxLoc(m_cornerStrength, &minStrength, &m_maxStrength);

      // local maxima detection
      cv::Mat dilated;  // temporary image
      cv::dilate(m_cornerStrength, dilated, cv::Mat());
      cv::compare(m_cornerStrength, dilated, m_localMax, cv::CMP_EQ);
}


cv::Mat HarrisDetector::getCornerMap(double qualityLevel)
{
      cv::Mat cornerMap;
      // thresholding the corner strength
      m_threshold = qualityLevel * m_maxStrength;
      cv::threshold(m_cornerStrength, m_cornerTh, m_threshold, 255, cv::THRESH_BINARY);
      // convert to 8-bit image
      m_cornerTh.convertTo(cornerMap, CV_8U);
      // non-maxima suppression
      cv::bitwise_and(cornerMap, m_localMax, cornerMap);

      return cornerMap;
}

void HarrisDetector::getCorners(std::vector<cv::Point> &points, double qualityLevel)
{
      // Get the corner map
      cv::Mat cornerMap= getCornerMap(qualityLevel);
      // Get the corners
      getCorners(points, cornerMap);
}

void HarrisDetector::getCorners(std::vector<cv::Point> &points, const cv::Mat& cornerMap)
{
    // Iterate over the pixels to obtain all feature points
    for( int y = 0; y < cornerMap.rows; y++ )
    {
        const uchar* rowPtr = cornerMap.ptr<uchar>(y);
        for( int x = 0; x < cornerMap.cols; x++ )
        {
            // if it is a feature point
            if (rowPtr[x])
            {
                points.push_back(cv::Point(x,y));
            }
        }
    }
}

void HarrisDetector::drawOnImage(cv::Mat &image, const std::vector<cv::Point> &points, cv::Scalar color, int radius, int thickness)
{
    std::vector<cv::Point>::const_iterator it= points.begin();
    // for all corners
    while (it!=points.end())
    {
        // draw a circle at each corner location
        cv::circle(image, *it, radius, color, thickness);
        ++it;
    }
}
